/*******************************************************************************
 * Copyright (c) 2012 K. Raizer.
 * All rights reserved. This program and the accompanying materials
 * are made available under the terms of the GNU Lesser Public License v2.1
 * which accompanies this distribution, and is available at
 * http://www.gnu.org/licenses/old-licenses/gpl-2.0.html
 * 
 * Contributors:
 *     K. Raizer- initial API and implementation
 ******************************************************************************/
import java.awt.BorderLayout;
import java.awt.Color;
import java.awt.image.BufferedImage;
import java.io.BufferedReader;
import java.io.IOException;
import java.io.InputStreamReader;
import java.io.PrintWriter;
import java.net.Socket;
import java.net.UnknownHostException;

import javax.swing.ImageIcon;
import javax.swing.JFrame;
import javax.swing.JLabel;
import javax.swing.JPanel;
import javax.swing.UIManager;
/**
 * 
 * @author Klaus
 *
 */

public class iCubMain {
	//with yarp name list I get:
	//// /icubSim/head/rpc:i ip 127.0.0.1 port 10222 type tcp
	private static int headPort=10192; //Port used for sending info
	private static int rootPort=10000; //root port
	private static String serverHostname="127.0.0.1";
	private static String[] imageArray;
	private static int[] image;
	protected static BufferedImage bufferedImage; // image (for GUI)
	protected static JLabel imageLabel; // swing widget for the image (GUI)
	protected JLabel batteryLabel;
	private static int cameraWidth=128;
	private static int cameraHeight=128;
	private static int index2;
	private static int index;
	/**
	 * @param args
	 */
	public static void main(String[] args) {
		Socket echoSocket = null;
		PrintWriter out = null;
		BufferedReader in = null;
		String message="";
		String response="";

		try {
			//FIRST PART -------------
			//			//First find out the desired port for a given address
			//			echoSocket = new Socket(serverHostname, rootPort);
			//			out = new PrintWriter(echoSocket.getOutputStream(), true);
			//			in = new BufferedReader(new InputStreamReader(echoSocket.getInputStream()));
			//
			//
			//			message="CONNECT foo";
			//			out.println(message);
			//			response=in.readLine();
			//
			//
			//			message="d";
			//			out.println(message);
			//			message="query /icubSim/head/rpc:i";
			//			out.println(message);
			//			response=in.readLine();
			//			String sentence[] = response.split(" ");
			//			if(sentence.length==9){
			//				headPort=
			//						Integer.valueOf(sentence[6]);
			//			}else{
			//				System.out.println("Something is wrong");
			//			}
			//
			//			//Second, connect to the desired port 
			//			echoSocket = new Socket(serverHostname, headPort);
			//			out = new PrintWriter(echoSocket.getOutputStream(), true);
			//			in = new BufferedReader(new InputStreamReader(echoSocket.getInputStream()));
			//
			//
			//			message="CONNECT foo";
			//			out.println(message);
			//			response=in.readLine();
			//
			//
			//			//Third, send commands to joints
			//
			//			message="d";
			//			out.println(message);
			//			message="set pos 0 30";
			//			out.println(message);
			//			response=in.readLine();		
			//			//Note: Robot's head should go up
			//
			//
			//			//Fourth, get information from encoders		
			//			message="d";
			//			out.println(message);
			//			message="get encs";
			//			out.println(message);
			//			response=in.readLine();
			//			System.out.println(response);
			//
			//			//Last, close connections
			//			out.close();
			//			in.close();
			//			echoSocket.close();

			//SECOND PART: let's get some images from vision

			//			Then from telnet:
			//				$ telnet localhost 10002
			//				Trying 127.0.0.1...
			//				Connected to localhost.
			//				Escape character is '^]'.
			//				CONNECT foo
			//				Welcome foo
			//				r
			//				do
			//				[mat] [rgb] 




			//First find out the desired port for a given address
			echoSocket = new Socket(serverHostname, rootPort);
			out = new PrintWriter(echoSocket.getOutputStream(), true);
			in = new BufferedReader(new InputStreamReader(echoSocket.getInputStream()));


			message="CONNECT foo";
			out.println(message);
			response=in.readLine();


			message="d";
			out.println(message);
			message="query /icubSim/cam/left/fovea";
			out.println(message);
			response=in.readLine();
			String sentence[] = response.split(" ");
			if(sentence.length==9){
				headPort=
						Integer.valueOf(sentence[6]);
			}else{
				System.out.println("Something is wrong");
			}

			//Second, connect to the desired port 
			echoSocket = new Socket(serverHostname, headPort);
			out = new PrintWriter(echoSocket.getOutputStream(), true);
			in = new BufferedReader(new InputStreamReader(echoSocket.getInputStream()));


			message="CONNECT foo";
			out.println(message);
			response=in.readLine();

			//Third, get information from encoders		
			message="r";
			out.println(message);
			response=in.readLine();
			response=in.readLine();
			response=response.replace("{","#");
			response=response.replace("}","#");
			String[] responseSplit = response.split("#");
			response=responseSplit[1];
//			System.out.println("4: "+response.substring(0, 100));
			imageArray=response.split(" ");
			

			image=new int[cameraHeight*cameraWidth];
			createWindow();
			index2=0;
			index=0;
			while(index<imageArray.length-3){		
				int r = Integer.valueOf(imageArray[index]);
				index++;
				int g = Integer.valueOf(imageArray[index]);
				index++;
				int b = Integer.valueOf(imageArray[index]);
				index++;
				
				Color cor= new Color(r, g, b);
				int value = cor.getRGB();
				image[index2]=value;
				index2++;
			}
			
			bufferedImage.setRGB(0,0,cameraWidth,cameraHeight,image,0,cameraWidth);
			imageLabel.repaint();
			
			
			//Last, close connections
			out.close();
			in.close();
			echoSocket.close();			












		} catch (UnknownHostException e) {
			// TODO Auto-generated catch block
			e.printStackTrace();

		} catch (IOException e) {
			// TODO Auto-generated catch block
			e.printStackTrace();
		}

	}
	public static void createWindow() {
		try {
			UIManager.setLookAndFeel(UIManager.getCrossPlatformLookAndFeelClassName());
		} catch (Exception e) {}
		JFrame frame = new JFrame("iCubVision");
		JPanel pane = new JPanel();
		final JLabel label = new JLabel("Vision");
		bufferedImage = new BufferedImage(cameraWidth,cameraHeight,
				BufferedImage.TYPE_INT_ARGB);
		imageLabel = new JLabel(new ImageIcon(bufferedImage));
		pane.add(imageLabel);
		pane.add(label);
		frame.getContentPane().add(pane,BorderLayout.CENTER);

		frame.pack();
		frame.setVisible(true);
	} 

}
